void MotorSetup()
{
    pinMode(Motor_RF, OUTPUT);
    pinMode(Motor_RB, OUTPUT);
    pinMode(Motor_LF, OUTPUT);
    pinMode(Motor_LB, OUTPUT);
}

void MotorSpeedUpdate()
{
  cli();
    //set left motor speed
    if (robotSpeed.LeftSpeed >= 0)
    {
      analogWrite(Motor_LF,robotSpeed.LeftSpeed*2);
      analogWrite(Motor_LB,0);
    }
    else 
    {
      if (robotSpeed.LeftSpeed == -128)
      {
        robotSpeed.LeftSpeed = -127;
      }
      analogWrite(Motor_LF,0);
      analogWrite(Motor_LB,abs(robotSpeed.LeftSpeed)*2);
    }
    
    //set right motor speed
    if (robotSpeed.RightSpeed >= 0)
    {
      analogWrite(Motor_RF,robotSpeed.RightSpeed*2);
      analogWrite(Motor_RB,0);
    }
    else 
    {
      if (robotSpeed.RightSpeed == -128)
      {
        robotSpeed.RightSpeed = -127;
      }
      analogWrite(Motor_RF,0);
      analogWrite(Motor_RB,abs(robotSpeed.RightSpeed)*2);
    }
    sei();
}    